package com.crunch.physics;

import com.crunch.math.Vector2f;

import java.awt.*;
import java.util.HashMap;
import java.util.Iterator;
import java.util.NoSuchElementException;

/**
 * Used for spatial partitioning of entities.
 *
 * The {@code HashGrid} class uses a hash map of grid cells to keep track of entities in space. The grid cells are of
 * fixed size, so this data structure works best with entities of similar size. An entity is defined using an
 * axis-aligned bounding box. Additionally, each entity is assigned a type, optional user-specified data, and can be
 * flagged as an "activator". An entity is an "activator" if it is capable of "activating" a grid cell for collision
 * checking when placed into that cell.
 *
 * When querying the grid for collision pairs, only cells in the "active cells" list are checked. A cell can only become
 * active if it contains at least one activator and at least two entities. This implies that non-activators can take
 * part in collisions, but cannot alone cause collisions. For example, suppose the player entity is an activator and
 * scene geometry entities are non-activators. In this case, a grid cell containing only scene geometry never needs to
 * be checked for collisions. As soon as the player enters the cell, it becomes active and collisions between the player
 * and scene geometry can be detected. When the player leaves the cell, it is once again deactivated. Similarly, if the
 * player is the only entity in a cell, that cell remains inactive.
 */
public class HashGrid {
	public static final int TYPES = 32;

	public HashGrid(float cellSize) {
		if (cellSize <= 0.0f) {
			throw new IllegalArgumentException("Invalid cell size");
		}

		nextId = 0;

		freeEntityList = null;
		activeCellList = null;
		freeCellList = null;
		freeCellEntityList = null;

		grid = new HashMap<Point, Cell>();
		invCellSize = 1.0f / cellSize;
	}

	// adds an entity to the grid and returns it
	public Entity addEntity(Vector2f aabbMin, Vector2f aabbMax, int type, boolean activator, Object data) {
		Entity entity;
		if (freeEntityList != null) {
			entity = freeEntityList;
			freeEntityList = entity.nextFreeEntity;
		} else {
			entity = new Entity();
		}

		// adds to cells automatically
		entity.reset(nextId, aabbMin, aabbMax, type, activator, data);
		++nextId;

		return entity;
	}

	// an entity in the grid
	public class Entity implements Comparable<Entity> {
		private Entity() {
			aabbMin = new Vector2f();
			aabbMax = new Vector2f();
		}

		// used internally to reuse entity
		private void reset(int sortId, Vector2f aabbMin, Vector2f aabbMax, int type, boolean activator, Object data) {
			this.sortId = sortId;

			this.aabbMin.set(aabbMin);
			this.aabbMax.set(aabbMax);

			if (type < 0 || type >= TYPES) {
				throw new IllegalArgumentException("Invalid type");
			}

			flags = (TYPE_BIT_MASK & type) | (activator ? ACTIVATOR_BIT_MASK : 0) | IN_USE_BIT_MASK;
			this.data = data;

			Point minCell = getMinCell(aabbMin);
			Point maxCell = getMaxCell(aabbMax);

			// add to cells
			for (int y = minCell.y; y < maxCell.y; ++y) {
				for (int x = minCell.x; x < maxCell.x; ++x) {
					addEntityToCell(this, x, y);
				}
			}
		}

		// removes this entity from the hash grid
		public void remove() {
			if ((flags & IN_USE_BIT_MASK) == 0) {
				throw new IllegalStateException("Entity not in use");
			}

			Point minCell = getMinCell(aabbMin);
			Point maxCell = getMaxCell(aabbMax);

			// remove from cells
			for (int y = minCell.y; y < maxCell.y; ++y) {
				for (int x = minCell.x; x < maxCell.x; ++x) {
					removeEntityFromCell(this, x, y);
				}
			}

			flags = 0;

			nextFreeEntity = freeEntityList;
			freeEntityList = this;
		}

		// moves this entity to a new position
		public void move(Vector2f aabbMin, Vector2f aabbMax) {
			if ((flags & IN_USE_BIT_MASK) == 0) {
				throw new IllegalStateException("Entity not in use");
			}

			Point oldMinCell = getMinCell(this.aabbMin);
			Point oldMaxCell = getMaxCell(this.aabbMax);
			Point newMinCell = getMinCell(aabbMin);
			Point newMaxCell = getMaxCell(aabbMax);

			Point isectMin = new Point(Math.max(oldMinCell.x, newMinCell.x), Math.max(oldMinCell.y, newMinCell.y));
			Point isectMax = new Point(Math.min(oldMaxCell.x, newMaxCell.x), Math.min(oldMaxCell.y, newMaxCell.y));

			// remove from old cells
			for (int y = oldMinCell.y; y < oldMaxCell.y; ++y) {
				for (int x = oldMinCell.x; x < oldMaxCell.x; ++x) {
					// don't touch intersection cells
					if (x < isectMin.x || y < isectMin.y || x >= isectMax.x || y >= isectMax.y) {
						removeEntityFromCell(this, x, y);
					}
				}
			}

			// add to new cells
			for (int y = newMinCell.y; y < newMaxCell.y; ++y) {
				for (int x = newMinCell.x; x < newMaxCell.x; ++x) {
					// don't touch intersection cells
					if (x < isectMin.x || y < isectMin.y || x >= isectMax.x || y >= isectMax.y) {
						addEntityToCell(this, x, y);
					}
				}
			}

			this.aabbMin.set(aabbMin);
			this.aabbMax.set(aabbMax);
		}

		// returns AABB min
		public Vector2f getAabbMin() {
			return new Vector2f(aabbMin);
		}

		// returns AABB max
		public Vector2f getAabbMax() {
			return new Vector2f(aabbMax);
		}

		// returns type
		public int getType() {
			return flags & TYPE_BIT_MASK;
		}

		// returns whether this entity is an activator
		public boolean isActivator() {
			return (flags & ACTIVATOR_BIT_MASK) != 0;
		}

		// returns data associated with entity
		public Object getData() {
			return data;
		}

		// sets data associated with entity
		public void setData(Object data) {
			this.data = data;
		}

		// used for sorting
		private int sortId;

		// AABB of the entity
		private Vector2f aabbMin;
		private Vector2f aabbMax;

		// entity flags:
		// bits [0,4] are the type
		// bit 5 is the activator flag
		// bit 6 is the in use flag
		private int flags;
		private static final int TYPE_BIT_MASK      = 0x1f;
		private static final int ACTIVATOR_BIT_MASK = 0x20;
		private static final int IN_USE_BIT_MASK    = 0x40;

		// data associated with entity
		private Object data;

		// next entity in free list
		private Entity nextFreeEntity;

		// used for sorting
		@Override
		public int compareTo(Entity o) {
			return Integer.compare(sortId, o.sortId);
		}
	}

	public static class CollisionPair implements Comparable<CollisionPair> {
		public CollisionPair(Entity entityA, Entity entityB) {
			this.entityA = entityA;
			this.entityB = entityB;
		}

		public Entity getEntityA() {
			return entityA;
		}

		public Entity getEntityB() {
			return entityB;
		}

		private Entity entityA, entityB;

		// used for sorting
		@Override
		public int compareTo(CollisionPair o) {
			int compA = entityA.compareTo(o.entityA);
			if (compA != 0) {
				return compA;
			} else {
				return entityB.compareTo(o.entityB);
			}
		}
	}

	public CollisionPairSet getCollisionPairSet(int typeAMask, int typeBMask) {
		return new CollisionPairSet(typeAMask, typeBMask);
	}

	// checks collisions between entities in the grid
	// a collision pair (eA, eB) is returned if:
	// - the AABBs of the two entities are intersecting
	// - at least one entity is an activator
	// - one entity is of a type in typeAMask and the other is of a type in typeBMask
	public class CollisionPairSet implements Iterable<CollisionPair> {
		private CollisionPairSet(int typeAMask, int typeBMask) {
			this.typeAMask = typeAMask;
			this.typeBMask = typeBMask;
		}

		@Override
		public Iterator<CollisionPair> iterator() {
			return new CollisionPairIterator(typeAMask, typeBMask);
		}

		private int typeAMask, typeBMask;
	}

	public class CollisionPairIterator implements Iterator<CollisionPair> {
		// private to avoid external instantiation
		private CollisionPairIterator(int typeAMask, int typeBMask) {
			isectMin = new Vector2f();
			isectMax = new Vector2f();

			this.typeAMask = typeAMask;
			this.typeBMask = typeBMask;

			// init pointers to data structure
			cell = activeCellList;
			if (cell != null) {
				eA = cell.entities;
				eB = (eA != null) ? eA.next : eB.next;
			} else {
				eA = eB = null;
			}

			next = advance();
		}

		@Override
		public boolean hasNext() {
			return (next != null);
		}

		@Override
		public CollisionPair next() {
			if (next == null) {
				throw new NoSuchElementException();
			}
			CollisionPair ret = next;
			next = advance();
			return ret;
		}

		@Override
		public void remove() {
			throw new UnsupportedOperationException();
		}

		private CollisionPair advance() {
			while (cell != null) {
				// find all potential colliding pairs within the cell
				while (eA != null) {
					boolean entityATypeA = ((1 << eA.entity.getType()) & typeAMask) != 0;
					boolean entityATypeB = ((1 << eA.entity.getType()) & typeBMask) != 0;

					// if neither mask is satisfied, no need to continue checking this entity
					if (entityATypeA || entityATypeB) {
						// start at next node to avoid duplicating work
						while (eB != null) {
							// make sure at least one is an activator
							if (eA.entity.isActivator() || eB.entity.isActivator()) {
								boolean entityBTypeA = ((1 << eB.entity.getType()) & typeAMask) != 0;
								boolean entityBTypeB = ((1 << eB.entity.getType()) & typeBMask) != 0;

								// make sure a type mask combination is satisfied: one entity must be in each mask
								if ((entityATypeA && entityBTypeB) || (entityATypeB && entityBTypeA)) {
									// check intersection
									isectMin.set(
											Math.max(eA.entity.aabbMin.x(), eB.entity.aabbMin.x()),
											Math.max(eA.entity.aabbMin.y(), eB.entity.aabbMin.y()));
									isectMax.set(
											Math.min(eA.entity.aabbMax.x(), eB.entity.aabbMax.x()),
											Math.min(eA.entity.aabbMax.y(), eB.entity.aabbMax.y()));
									if (isectMin.x() <= isectMax.x() && isectMin.y() <= isectMax.y()) {
										CollisionPair pair = new CollisionPair(eA.entity, eB.entity);
										// advance eB before returning
										eB = eB.next;
										return pair;
									}
								}
							}
							eB = eB.next;
						}
					}
					eA = eA.next;
					eB = (eA != null) ? eA.next : null;
				}
				cell = cell.nextActiveOrFreeCell;
				if (cell != null) {
					eA = cell.entities;
					eB = (eA != null) ? eA.next : null;
				} else {
					eA = eB = null;
				}
			}

			return null;
		}

		private Vector2f isectMin;
		private Vector2f isectMax;

		private int typeAMask;
		private int typeBMask;

		private Cell cell;
		private CellEntity eA;
		private CellEntity eB;
		private CollisionPair next;
	}

	public CollisionAabbSet getCollisionAabbSet(int typeMask, Vector2f aabbMin, Vector2f aabbMax) {
		return new CollisionAabbSet(typeMask, aabbMin, aabbMax);
	}

	// checks collisions between entities in the grid and the given AABB
	// non-activators can be returned in this case
	public class CollisionAabbSet implements Iterable<Entity> {
		private CollisionAabbSet(int typeMask, Vector2f aabbMin, Vector2f aabbMax) {
			this.typeMask = typeMask;
			this.aabbMin = new Vector2f(aabbMin);
			this.aabbMax = new Vector2f(aabbMax);
		}

		@Override
		public Iterator<Entity> iterator() {
			return new CollisionAabbIterator(typeMask, aabbMin, aabbMax);
		}

		private int typeMask;
		private Vector2f aabbMin;
		private Vector2f aabbMax;
	}

	public class CollisionAabbIterator implements Iterator<Entity> {
		// private to avoid external instantiation
		private CollisionAabbIterator(int typeMask, Vector2f aabbMin, Vector2f aabbMax) {
			isectMin = new Vector2f();
			isectMax = new Vector2f();

			this.aabbMin = new Vector2f(aabbMin);
			this.aabbMax = new Vector2f(aabbMax);

			this.minX = (int) Math.floor(aabbMin.x() * invCellSize);
			this.minY = (int) Math.floor(aabbMin.y() * invCellSize);
			this.maxX = (int) Math.ceil(aabbMax.x() * invCellSize);
			this.maxY = (int) Math.ceil(aabbMax.y() * invCellSize);

			this.typeMask = typeMask;

			cellIndex = new Point(minX, minY);

			Cell cell = grid.get(cellIndex);
			e = (cell != null) ? cell.entities : null;

			next = advance();
		}

		@Override
		public boolean hasNext() {
			return (next != null);
		}

		@Override
		public Entity next() {
			if (next == null) {
				throw new NoSuchElementException();
			}
			Entity ret = next;
			next = advance();
			return ret;
		}

		@Override
		public void remove() {
			throw new UnsupportedOperationException();
		}

		private Entity advance() {
			while (cellIndex.y < maxY) {
				while (cellIndex.x < maxX) {
					while (e != null) {
						// check for valid type
						if (((1 << e.entity.getType()) & typeMask) != 0) {
							isectMin.set(
									Math.max(e.entity.aabbMin.x(), aabbMin.x()),
									Math.max(e.entity.aabbMin.y(), aabbMin.y()));
							isectMax.set(
									Math.min(e.entity.aabbMax.x(), aabbMax.x()),
									Math.min(e.entity.aabbMax.y(), aabbMax.y()));
							if (isectMin.x() <= isectMax.x() && isectMin.y() <= isectMax.y()) {
								Entity entity = e.entity;
								// advance eB before returning
								e = e.next;
								return entity;
							}
						}

						e = e.next;
					}

					++cellIndex.x;
					if (cellIndex.x < maxX) {
						Cell cell = grid.get(cellIndex);
						e = (cell != null) ? cell.entities : null;
					} else {
						e = null;
					}
				}

				++cellIndex.y;
				if (cellIndex.y < maxY) {
					cellIndex.x = minX;
					Cell cell = grid.get(cellIndex);
					e = (cell != null) ? cell.entities : null;
				} else {
					e = null;
				}
			}

			return null;
		}

		private Vector2f isectMin;
		private Vector2f isectMax;
		private Vector2f aabbMin;
		private Vector2f aabbMax;

		private int minX, minY;
		private int maxX, maxY;

		private int typeMask;

		private Point cellIndex;
		private CellEntity e;
		private Entity next;
	}

	// used to assign unique ID to new entities for sorting
	private int nextId;

	// list of free entities that can be reused
	private Entity freeEntityList;

	// cell in the grid
	private static class Cell {
		// list of entities in the cell
		public CellEntity entities;

		// number of activators in the cell
		public int activatorCount;

		// pointer to next active cell in the active cell list
		public Cell prevActiveCell;
		// pointer to prev active cell in the active cell list, or next free cell
		public Cell nextActiveOrFreeCell;

		// active if at least 1 activator and at least 2 objects
		public boolean shouldBeActive() {
			// if activatorCount >= 1, then entities is not null, so it is safe to check entities.next
			return activatorCount >= 1 && entities.next != null;
		}
	}

	// list of cells containing at least 2 activators
	private Cell activeCellList;
	// list of free cells that can be reused
	private Cell freeCellList;

	private static class CellEntity {
		// the entity
		public Entity entity;
		// next entity in the cell, or next free instance
		public CellEntity next;
	}

	private CellEntity freeCellEntityList;

	// hash map of grid cells
	private HashMap<Point, Cell> grid;
	// 1/size of cells
	private float invCellSize;

	private void addEntityToCell(Entity entity, int cx, int cy) {
		CellEntity cellEntity;
		if (freeCellEntityList != null) {
			cellEntity = freeCellEntityList;
			freeCellEntityList = cellEntity.next;
		} else {
			cellEntity = new CellEntity();
		}
		cellEntity.entity = entity;

		Point point = new Point(cx, cy);
		Cell cell = grid.get(point);
		boolean wasActive;
		if (cell == null) {
			// create new cell and add as first entry
			if (freeCellList != null) {
				cell = freeCellList;
				freeCellList = cell.nextActiveOrFreeCell;
			} else {
				cell = new Cell();
			}
			cellEntity.next = null;
			cell.entities = cellEntity;
			cell.activatorCount = 0;
			cell.prevActiveCell = null;
			cell.nextActiveOrFreeCell = null;
			grid.put(point, cell);
			wasActive = false;
		} else {
			wasActive = cell.shouldBeActive();
			// add to front of existing node's entity list
			cellEntity.next = cell.entities;
			cell.entities = cellEntity;
		}

		// if the entity is an activator, increment the activator count and possibly activate the cell
		if (entity.isActivator()) {
			++cell.activatorCount;
		}

		boolean isActive = cell.shouldBeActive();
		if (!wasActive && isActive) {
			// activate the cell by putting it in the active cell list
			// prev is already null
			cell.nextActiveOrFreeCell = activeCellList;
			if (cell.nextActiveOrFreeCell != null) {
				// cell.next.prev = cell
				cell.nextActiveOrFreeCell.prevActiveCell = cell;
			}
			activeCellList = cell;
		}
	}

	private void removeEntityFromCell(Entity entity, int cx, int cy) {
		Point point = new Point(cx, cy);
		Cell cell = grid.get(point);
		boolean wasActive = cell.shouldBeActive();

		// search the cell list until the node is found
		CellEntity prev = null;
		CellEntity curr = cell.entities;
		while (curr != null) {
			if (curr.entity == entity) {
				// found the node with the entity - remove it
				if (prev == null) {
					// adjust head of list
					cell.entities = curr.next;
				} else {
					// adjust previous node in list
					prev.next = curr.next;
				}

				// free the cell entity
				curr.entity = null;
				curr.next = freeCellEntityList;
				freeCellEntityList = curr;

				// if the entity is an activator, decrement activator count
				if (entity.isActivator()) {
					--cell.activatorCount;
				}
				// check if this cell is no longer an activator
				boolean isActive = cell.shouldBeActive();
				if (wasActive && !isActive) {
					// remove cell from active list
					if (cell.prevActiveCell == null) {
						activeCellList = cell.nextActiveOrFreeCell;
					} else {
						cell.prevActiveCell.nextActiveOrFreeCell = cell.nextActiveOrFreeCell;
					}
					if (cell.nextActiveOrFreeCell != null) {
						cell.nextActiveOrFreeCell.prevActiveCell = cell.prevActiveCell;
					}
					cell.prevActiveCell = cell.nextActiveOrFreeCell = null;
				}

				// remove the cell if it is now empty
				if (cell.entities == null) {
					grid.remove(point);
					cell.nextActiveOrFreeCell = freeCellList;
					freeCellList = cell;
				}

				return;
			}

			prev = curr;
			curr = curr.next;
		}

		throw new IllegalStateException("Entity not in cell");
	}

	private Point getMinCell(Vector2f aabbMin) {
		return new Point(
				(int) Math.floor(aabbMin.x() * invCellSize),
				(int) Math.floor(aabbMin.y() * invCellSize));
	}

	private Point getMaxCell(Vector2f aabbMax) {
		return new Point(
				(int) Math.ceil(aabbMax.x() * invCellSize),
				(int) Math.ceil(aabbMax.y() * invCellSize));
	}
}
